/*
* @File Name collide_top.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\task\task_collide\collide_top.c
* @Author: Ruige_Lee
* @Date:   2019-08-20 20:59:25
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-10-06 16:13:32
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/

#include <stdint.h>
#include "xil_printf.h"

#include "stepper/steppers.h"

#include "task/include.h"
#include "route/realTimePrm/obstacle/collide.h"
#include "route/realTimePrm/prm.h"
#include "math/fast_math.h"

extern uint32_t profileCnt;
float endConfig[6][6] = {
		{0,  -1.77,1.57,0,1.57,0},
		{0.75,-1.77,1.57,0,1.57,0},
		{0.75,-1.35,1.57,0,1.57,0},
		{0,   -1.35,1.57,0,1.57,0},
		{-0.75,-1.35,1.57,0,1.57,0},
		{-0.75,-1.77,1.57,0,1.57,0}
	};

static void obs_arm_move()
{
	if (FIFO_empty[1] == 1)
	{
		static uint8_t endCnt = 0;
	
		//障碍机械臂随机位置
		float startConfig[6];
		
		
		Cspline[1].count = 3;
		
		for (uint8_t axis = 0; axis < 6; axis ++)
		{
			//get perious place
			startConfig[axis] = stepper_getRad( 1, axis );
			Cspline[1].points[0].position[axis] = startConfig[axis];
			Cspline[1].points[1].position[axis] = ( endConfig[endCnt][axis] + startConfig[axis] ) / 2;
			Cspline[1].points[2].position[axis] = endConfig[endCnt][axis];
		}
		tra_autoSet_timePoint( 1 );
		tra_checkIn(1);

		endCnt ++;
		if ( endCnt >= 6 )
		{
			endCnt = 0;
		}
	}
}

float prmBotPose[6] = {0.,0.,0.,0.,0.,0.};
uint8_t rePlaning = 1;
extern float poseTable[2431][6];
extern uint32_t checkArrayPoint;
extern uint32_t checkArrayMax;


void task_collide()
{
	//模拟障碍机械臂随机运动
	obs_arm_move();
	

	float staPose[6];
	float distanceSum = 0;

	for (uint8_t axis = 0; axis < 6; axis ++)
	{
		staPose[axis] = stepper_getRad( 0, axis );
		distanceSum += fast_abs(prmBotPose[axis]-staPose[axis]);
	}

	if ( checkArrayPoint >= checkArrayMax ) //路线执行完毕
	{
		uint16_t sw = (uint16_t)(rand() / 2147483647. * 2431);

		for ( uint8_t axis = 0; axis < 6; axis ++ )
		{
			prmBotPose[axis] = poseTable[sw][axis];
		}
		rePlaning = 1;
	}

	else
	{
		rePlaning = 0;
	}
	
	




}




